/*
 * ins.c
 *
 *  Created on: 2022.2.26
 *      Author: huan
 */

#include "head_user.h"
#include "head_rtthread.h"
#include <math.h>

INS_HandleTypedef INS_Handle;
static char mb_pool[4];
static struct rt_mailbox mb_Euler;
static IMU_st imu_data = {1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};

static void AHRS_update(float dt,const Axis3f gyro_f,const Axis3f acce_f,const Axis3f magn_f,IMU_st*imu);

void AttitudeSolution_Task(void* parameter)
{
    imuData_t *imu;
    rt_ubase_t address;
    rt_err_t result;
    static uint32_t tick;
    /* initialize a mailbox */
    result = rt_mb_init(&mb_Euler, "mb_Euler", &mb_pool[0], 1, RT_IPC_FLAG_FIFO);
    if (result != RT_EOK)
    {
        rt_kprintf("mb_Euler has a problem!\r\n");
    }
    while(1)
    {
        rt_mb_recv(IMU_Handle.mb, &address, RT_WAITING_FOREVER);
        imu = (imuData_t *)address;

        /* attitude solution */
        AHRS_update(0.001, imu->gyro, imu->acce, imu->acce, &imu_data);
        /* data fusion */
        if(tick%2 == 0)
        {
            Position_Estimation(0.002);     //500HZ
        }
        tick++;
    }
}

/*-----------------------------------------------------------------------------------------------------------------------*/
/* 姿态解算 */
#define long_ku_2   0   /* Range_Kuta-2 solving differential equation */
#define long_ku_1   0   /* Range_Kuta-1 solving differential equation */
#define bika_2      0   /* bika-2 solving differential equation */
#define bika_1      1   /* bika-1 solving differential equation */

#define USE_Magn    1
#define Deg_PerRAD  57.29577f

#define KP_Acce     0.3f    /* accelerometer compensation gyroscope P_I*/
#define KI_Acce     0.01f   /* accelerometer compensation gyroscope P_I*/

#define KP_Magn     1.0f    /* magnetometer compensation gyroscope P_I*/
#define Ki_Magn     0.0f    /* magnetometer compensation gyroscope P_I*/

#define Spin_Rate_Limit     20.0f     /* accelerometer compensation gyroscope limit��degree/second*/

static float rMat[3][3];              /*rotation matrix*/

static void AHRS_update(float dt,const Axis3f gyro_f,const Axis3f acce_f,const Axis3f magn_f,IMU_st *imu)
{
    float q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
    float d_angle[3], spin_rate_sq;

    Axis3f gyro, acce, acce_unit, vector_err = {.x =0,.y =0,.z =0};
    static Axis3f acce_inte = {.x =0,.y =0,.z =0};

    for(uint8_t i = 0; i < 3; i++)
    {
        acce.axis[i] = acce_f.axis[i] / 100;         //acceleration unit conversion:m/s2
    }
    for(uint8_t i = 0; i < 3; i++)
    {
        gyro.axis[i] = gyro_f.axis[i] / Deg_PerRAD;  //angular velocity unit conversion:rad/s
    }
    /* calculate rotation rate */
    spin_rate_sq = powf(gyro.x,2) + powf(gyro.y,2) + powf(gyro.z,2);
#if USING_Magnetometer
        const float magMagnitudeSq = magn.x * magn.x + magn.y * magn.y  + magn.z * magn.z;
        float kpMag = KP_Magn*0;
         if (magMagnitudeSq > 0.01f)
         {
             const float magRecipNorm = sqrt_reciprocal(magMagnitudeSq);
             magn.x *= magRecipNorm;
             magn.y *= magRecipNorm;
             magn.z *= magRecipNorm;

             hx = rMat[0][0] * magn.x + rMat[0][1] * magn.y + rMat[0][2] * magn.z;
             hy = rMat[1][0] * magn.x + rMat[1][1] * magn.y + rMat[1][2] * magn.z;
             bx =hx * hx + hy * hy;

             ez_ef = -(hy * bx);

             vec_err_x = rMat[2][0] * ez_ef;
             vec_err_y = rMat[2][1] * ez_ef;
             vec_err_z = rMat[2][2] * ez_ef;
         }
         else
         {
             vec_err_x = 0;
             vec_err_y = 0;
             vec_err_z = 0;
         }
         if (Ki_Magn > 0.0f)
         {
             if (spin_rate_sq < square(Spin_Rate_Limit*RAD_PER_DEG))
             {
                 Mag_inte_x += Ki_Magn * vec_err_x * dt;
                 Mag_inte_y += Ki_Magn * vec_err_y * dt;
                 Mag_inte_z += Ki_Magn * vec_err_z * dt;

                 gyro.x += Mag_inte_x;
                 gyro.y += Mag_inte_y;
                 gyro.z += Mag_inte_z;
             }
         }
         gyro.x += kpMag * vec_err_x;
         gyro.y += kpMag * vec_err_y;
         gyro.z += kpMag * vec_err_z;
#endif

    /* if acceleration data is valid, compensate gyroscope with acceletometer*/
    if((acce.x != 0.0f) || (acce.y != 0.0f) || (acce.z != 0.0f))
    {
        /* square toot, unitization */
        const float acce_sqrt = sqrtf(powf(acce.x,2)+powf(acce.y,2)+powf(acce.z,2));
        //g_debug_x[0] = (int)(acce_sqrt*100);
        for(uint8_t i = 0; i <3; i++)
        {
            acce_unit.axis[i] = acce.axis[i] / acce_sqrt;
        }
        /* vector error for compensation gyroscope */
        vector_err.x =   (acce_unit.axis[2] * rMat[2][1] - rMat[2][2] * acce_unit.axis[1]);
        vector_err.y =   (acce_unit.axis[0] * rMat[2][2] - rMat[2][0] * acce_unit.axis[2]);
        vector_err.z =   (acce_unit.axis[1] * rMat[2][0] - rMat[2][1] * acce_unit.axis[0]);

        /* no compensation if accelerometer is out of G*/
        if(acce_sqrt > 10.0f || acce_sqrt < 9.5f)
        {
            vector_err.x = vector_err.y = vector_err.z = 0;
        }
        /* cumulation error compensation */
        /* cumulation is only made when the rotation rate is less than a limit */
        if(spin_rate_sq < powf(Spin_Rate_Limit/Deg_PerRAD,2))
        {
            acce_inte.x += limit(vector_err.x,-0.1f,0.1f) *dt*KI_Acce;
            acce_inte.y += limit(vector_err.y,-0.1f,0.1f) *dt*KI_Acce;
            acce_inte.z += limit(vector_err.z,-0.1f,0.1f) *dt*KI_Acce;
        }
    }
    d_angle[0] =  (gyro.x + vector_err.x * KP_Acce + acce_inte.x) * dt / 2 ;
    d_angle[1] =  (gyro.y + vector_err.y * KP_Acce + acce_inte.y) * dt / 2 ;
    d_angle[2] =  (gyro.z + vector_err.z * KP_Acce + acce_inte.z) * dt / 2 ;
#if (bika_1)
    /* bika-1 solving differential equation */
    imu->q0 = imu->q0            - imu->q1*d_angle[0] - imu->q2*d_angle[1] - imu->q3*d_angle[2];
    imu->q1 = imu->q0*d_angle[0] + imu->q1            + imu->q2*d_angle[2] - imu->q3*d_angle[1];
    imu->q2 = imu->q0*d_angle[1] - imu->q1*d_angle[2] + imu->q2            + imu->q3*d_angle[0];
    imu->q3 = imu->q0*d_angle[2] + imu->q1*d_angle[1] - imu->q2*d_angle[0] + imu->q3;
#endif

#if (bika_2)
    /* bika-2 solving differential equation */
    float delta = (dt/2 *gyro.x) * (dt/2 * gyro.x) + (dt/2 * gyro.y) * (dt/2 * gyro.y) + (dt/2* gyro.z) * (dt/2 * gyro.z);
    imu->q0 = (1.0f - delta / 8.0f) * imu->q0 + (-imu->q1*  gyro.x - imu->q2*gyro.y - imu->q3*gyro.z)* dt/2;
    imu->q1 = (1.0f - delta / 8.0f) * imu->q1 + ( imu->q0*  gyro.x + imu->q2*gyro.z - imu->q3*gyro.y)* dt/2;
    imu->q2 = (1.0f - delta / 8.0f) * imu->q2 + ( imu->q0*  gyro.y - imu->q1*gyro.z + imu->q3*gyro.x)* dt/2;
    imu->q3 = (1.0f - delta / 8.0f) * imu->q3 + ( imu->q0*  gyro.z + imu->q1*gyro.y - imu->q2*gyro.x)* dt/2;
#endif

#if (long_ku_1)
    /* Range_Kuta-1 solving differential equation */
    imu->q0 += (-imu->q1 * gyro.x - imu->q2 * gyro.y - imu->q3 * gyro.z) * dt/2;
    imu->q1 += ( imu->q0 * gyro.x + imu->q2 * gyro.z - imu->q3 * gyro.y) * dt/2;
    imu->q2 += ( imu->q0 * gyro.y - imu->q1 * gyro.z + imu->q3 * gyro.x) * dt/2;
    imu->q3 += ( imu->q0 * gyro.z + imu->q1 * gyro.y - imu->q2 * gyro.x) * dt/2;

#endif

#if (long_ku_2)
    /* Range_Kuta-2 solving differential equation */
    dt = dt/2.0f;
    float k11= (-imu->q1 * gyro.x - imu->q2 * gyro.y - imu->q3 * gyro.z) * dt;
    float k12= ( imu->q0 * gyro.x + imu->q2 * gyro.z - imu->q3 * gyro.y) * dt;
    float k13= ( imu->q0 * gyro.y - imu->q1 * gyro.z + imu->q3 * gyro.x) * dt;
    float k14= ( imu->q0 * gyro.z + imu->q1 * gyro.y - imu->q2 * gyro.x) * dt;

    float k21= ((-imu->q1+k11) * (gyro.x+dt) - (imu->q2+k11) * (gyro.y+dt) - (imu->q3+k11) * (gyro.z+dt)) * dt;
    float k22= (( imu->q0+k12) * (gyro.x+dt) + (imu->q2+k12) * (gyro.z+dt) - (imu->q3+k12) * (gyro.y+dt)) * dt;
    float k23= (( imu->q0+k13) * (gyro.y+dt) - (imu->q1+k13) * (gyro.z+dt) + (imu->q3+k13) * (gyro.x+dt)) * dt;
    float k24= (( imu->q0+k14) * (gyro.z+dt) + (imu->q1+k14) * (gyro.y+dt) - (imu->q2+k14) * (gyro.x+dt)) * dt;

    imu->q0 += ( k11+k21)/2;
    imu->q1 += ( k12+k22)/2;
    imu->q2 += ( k13+k23)/2;
    imu->q3 += ( k24+k14)/2;
#endif
    /* quaternion unitization */
    const float Q_sqrt = sqrtf(powf(imu->q0,2)+powf(imu->q1,2)+powf(imu->q2,2)+powf(imu->q3,2));
    imu->q0 /= Q_sqrt;
    imu->q1 /= Q_sqrt;
    imu->q2 /= Q_sqrt;
    imu->q3 /= Q_sqrt;
    /* quaternion to matrix */
    q0q1 = imu->q0 * imu->q1;
    q0q2 = imu->q0 * imu->q2;
    q0q3 = imu->q0 * imu->q3;

    q1q1 = imu->q1 * imu->q1;
    q1q2 = imu->q1 * imu->q2;
    q1q3 = imu->q1 * imu->q3;

    q2q2 = imu->q2 * imu->q2;
    q2q3 = imu->q2 * imu->q3;
    q3q3 = imu->q3 * imu->q3;


    /* 旋转矩阵, 载体系到导航系坐标变换矩阵 */
    rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
    rMat[0][1] = 2.0f * (q1q2-q0q3);
    rMat[0][2] = 2.0f * (q1q3+q0q2);

    rMat[1][0] = 2.0f * (q1q2 +q0q3);
    rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
    rMat[1][2] = 2.0f * (q2q3-q0q1);

    rMat[2][0] = 2.0f * (q1q3-q0q2);
    rMat[2][1] = 2.0f * (q2q3+q0q1);
    rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;


    /* solving attitude angle */
    imu->pitch  =   asinf(-rMat[2][0]            )*Deg_PerRAD;    //绕y轴转动角
    imu->roll   =   atan2f(rMat[2][1], rMat[2][2])*Deg_PerRAD;    //绕x轴转动角
    imu->yaw    =   atan2f(rMat[1][0], rMat[0][0])*Deg_PerRAD;    //绕z轴转动角
}
/*-----------------------------------------------------------------------------------------------------------------------*/


static int ins_init(void)
{
    INS_Handle.imuAngle   = &imu_data;
    INS_Handle.rMat       = rMat;
    INS_Handle.mb         = &mb_Euler;

    return RT_EOK;
}

INIT_PREV_EXPORT(ins_init);
